<!DOCTYPE html PUBLIC "-//W3C//DTD HTML 4.01 Transitional//EN">
<html><head><meta http-equiv="Content-Type" content="text/html; charset=utf-8"><meta http-equiv="X-UA-Compatible" content="IE=edge,IE=9,chrome=1"><meta name="generator" content="MATLAB 2021b"><title>WBR控制</title><style type="text/css">.rtcContent { padding: 30px; } .S0 { margin: 3px 10px 5px 4px; padding: 0px; line-height: 28.8px; min-height: 0px; white-space: pre-wrap; color: rgb(213, 80, 0); font-family: Helvetica, Arial, sans-serif; font-style: normal; font-size: 24px; font-weight: 400; text-align: left;  }
.S1 { margin-bottom: 20px; padding-bottom: 4px;  }
.S2 { margin: 0px; padding: 10px 0px 10px 5px; line-height: 21px; min-height: 0px; white-space: pre-wrap; color: rgb(0, 0, 0); font-family: Helvetica, Arial, sans-serif; font-style: normal; font-size: 14px; font-weight: 700; text-align: start;  }
.S3 { margin: -1px 0px 0px; padding: 10px 0px 10px 7px; line-height: 21px; min-height: 0px; white-space: pre-wrap; color: rgb(0, 0, 0); font-family: Helvetica, Arial, sans-serif; font-style: normal; font-size: 14px; font-weight: 400; text-align: start;  }
.S4 { margin: 20px 10px 5px 4px; padding: 0px; line-height: 20px; min-height: 0px; white-space: pre-wrap; color: rgb(60, 60, 60); font-family: Helvetica, Arial, sans-serif; font-style: normal; font-size: 20px; font-weight: 700; text-align: left;  }
.S5 { margin: 2px 10px 9px 4px; padding: 0px; line-height: 21px; min-height: 0px; white-space: pre-wrap; color: rgb(0, 0, 0); font-family: Helvetica, Arial, sans-serif; font-style: normal; font-size: 14px; font-weight: 400; text-align: left;  }
.S6 { margin: 3px 10px 5px 4px; padding: 0px; line-height: 20px; min-height: 0px; white-space: pre-wrap; color: rgb(60, 60, 60); font-family: Helvetica, Arial, sans-serif; font-style: normal; font-size: 20px; font-weight: 700; text-align: left;  }
.S7 { margin: 15px 10px 5px 4px; padding: 0px; line-height: 18px; min-height: 0px; white-space: pre-wrap; color: rgb(60, 60, 60); font-family: Helvetica, Arial, sans-serif; font-style: normal; font-size: 17px; font-weight: 700; text-align: left;  }
.S8 { margin: 10px 10px 9px 4px; padding: 0px; line-height: 21px; min-height: 0px; white-space: pre-wrap; color: rgb(0, 0, 0); font-family: Helvetica, Arial, sans-serif; font-style: normal; font-size: 14px; font-weight: 400; text-align: left;  }
.S9 { margin: 3px 10px 5px 4px; padding: 0px; line-height: 18px; min-height: 0px; white-space: pre-wrap; color: rgb(60, 60, 60); font-family: Helvetica, Arial, sans-serif; font-style: normal; font-size: 17px; font-weight: 700; text-align: left;  }</style></head><body><div class = rtcContent><h1  class = 'S0' id = 'T_7CAB6855' ><span>WBR控制</span></h1><div  class = 'S1'><div  class = 'S2'><span style=' font-weight: bold;'>目录</span></div><div  class = 'S3'><a href = "#H_E9464950"><span>0 准备工作
</span></a><a href = "#H_7DFB32B1"><span>1 机器人参数
</span></a><a href = "#H_8FA9B9F9"><span>2 模型实例化
</span></a><span>    </span><a href = "#H_A1DE8800"><span>2.1 状态空间模型
</span></a><span>    </span><a href = "#H_DAA261DE"><span>2.2 腿部模型
</span></a><a href = "#H_5DD42F05"><span>3 控制器设计
</span></a><span>    </span><a href = "#H_52535545"><span>3.1 运动控制器（Locomotion Controller）
</span></a><span>    </span><a href = "#H_913C7C83"><span>3.2 腿部控制器（Leg Controller）
</span></a><a href = "#H_31305A8B"><span>4 simulink仿真</span></a></div></div><h2  class = 'S4' id = 'H_E9464950' ><span>0 准备工作</span></h2><div  class = 'S5'><a href = "./WBR_modeling.html"><span>WBR系统建模分析</span></a></div><div  class = 'S5'><a href = "./WBR_leg.html"><span>WBR腿部五连杆机构分析</span></a></div><h2  class = 'S6' id = 'H_7DFB32B1' ><span>1 机器人参数</span></h2><div  class = 'S5'><span>机器人质量、尺寸参数通过机械图纸测量得到，其中腿部惯量和质心位置简化近似为腿长 </span><span style="font-family: STIXGeneral, STIXGeneral-webfont, serif; font-style: italic; font-weight: 400; color: rgb(0, 0, 0);">l</span><span> 的线性函数</span></div><div  class = 'S5'><span>单侧腿平面转动惯量 </span></div><div  class = 'S5'><span texencoding="I_l\approx0.4l+0.07\ (kg\cdot m^2)" style="vertical-align:-6px"><img src="" width="154.5" height="20.5" /></span><span>    (1.1)</span></div><div  class = 'S5'><span>腿部质心位置</span></div><div  class = 'S5'><span texencoding="l_b\approx0.218l+0.075\ (m)" style="vertical-align:-6px"><img src="" width="149" height="19.5" /></span><span>    (1.2)</span></div><div  class = 'S5'><span texencoding="l_w=l-l_b\approx0.782l-0.075\ (m)" style="vertical-align:-6px"><img src="" width="199.5" height="19.5" /></span><span>    (1.3)</span></div><h2  class = 'S6' id = 'H_8FA9B9F9' ><span>2 模型实例化</span></h2><div  class = 'S5'><span>将机器人的具体参数代入模型中得到实例化的模型</span></div><h3  class = 'S7' id = 'H_A1DE8800' ><span>2.1 状态空间模型</span></h3><div  class = 'S8'><span>为降低计算量，通过多项式拟合得到状态方程中矩阵 A, B 关于腿长 </span><span texencoding="(l_l,l_r)" style="vertical-align:-6px"><img src="" width="36.5" height="19.5" /></span><span> 的近似函数。若计算资源紧张可利用矩阵 A, B 的稀疏特性进一步降低计算量。</span></div><div  class = 'S5'><span texencoding="A(l_l,l_r)=K_{A,1}+K_{A,2}l_l+K_{A,3}l_l^2+K_{A,4}l_r+K_{A,5}l_r^2+K_{A,6}l_ll_r" style="vertical-align:-8px"><img src="" width="347.5" height="21.5" /></span><span>    (2.1)</span></div><div  class = 'S5'><span texencoding="B(l_l,l_r)=K_{B,1}+K_{B,2}l_l+K_{B,3}l_l^2+K_{B,4}l_r+K_{B,5}l_r^2+K_{B,6}l_ll_r" style="vertical-align:-8px"><img src="" width="344" height="21.5" /></span><span>    (2.2)</span></div><h3  class = 'S9' id = 'H_DAA261DE' ><span>2.2 腿部模型</span></h3><div  class = 'S5'><span>由于腿部连杆正运动学计算较为复杂，因此在工作空间内采用多项式拟合得到正运动学的近似结果降低计算量</span></div><div  class = 'S5'><span texencoding="x_e(\varphi_1,\varphi_2)=K_{xe,1}+K_{xe,2}\varphi_1+K_{xe,3}\varphi_2+K_{xe,4}\varphi_1^2+K_{xe,5}\varphi_2^2+K_{xe,6}\varphi_1\varphi_2" style="vertical-align:-8px"><img src="" width="411" height="21.5" /></span><span>    (2.3)</span></div><div  class = 'S5'><span texencoding="y_e(\varphi_1,\varphi_2)=K_{ye,1}+K_{ye,2}\varphi_1+K_{ye,3}\varphi_2+K_{ye,4}\varphi_1^2+K_{ye,5}\varphi_2^2+K_{ye,6}\varphi_1\varphi_2" style="vertical-align:-8px"><img src="" width="412" height="21.5" /></span><span>    (2.4)</span></div><h2  class = 'S6' id = 'H_5DD42F05' ><span>3 控制器设计</span></h2><div  class = 'S5'><span>机器人控制解耦为运动控制（Locomotion Controller）和腿部控制（Leg Controller）两部分。运动控制器通过调节驱动轮转矩和腿部关节转矩使机器人保持平衡和控制机器人移动、旋转；腿部控制器通过调节腿部支持力控制腿长，使机器人保持滚转（roll）方向稳定，并起到主动悬挂的作用。</span></div><h3  class = 'S7' id = 'H_52535545' ><span>3.1 运动控制器（Locomotion Controller）</span></h3><div  class = 'S5'><span>根据状态空间模型设计状态反馈控制器</span></div><div  class = 'S5'><span>状态反馈控制器</span></div><div  class = 'S5'><span texencoding="\mathbf{u}=K_{4\times10}(\mathbf{x}_d-\mathbf{x})" style="vertical-align:-6px"><img src="" width="111.5" height="19.5" /></span><span>    (3.1)</span></div><div  class = 'S5'><span>状态反馈矩阵 K 通过 Linear Quadratic Regulator (LQR) 计算得到，代价函数</span></div><div  class = 'S5'><span texencoding="J=\int_{0}^{\infty}\left(x^TQx+u^TRu\right)dx" style="vertical-align:-8px"><img src="" width="158" height="26" /></span><span>    (3.2)</span></div><div  class = 'S5'><span>根据 LQR 可以得到状态反馈矩阵 K</span></div><div  class = 'S5'><span texencoding="K=R^{-1}B^TP" style="vertical-align:-5px"><img src="" width="80.5" height="18.5" /></span><span>    (3.3)</span></div><div  class = 'S5'><span>其中 P 满足 Riccati 方程</span></div><div  class = 'S5'><span texencoding="A^TP+PA+PBR^{-1}B^TP+Q=0" style="vertical-align:-5px"><img src="" width="200.5" height="18.5" /></span><span>    (3.4)</span></div><div  class = 'S5'><span>式 (3.4) 中，</span><span texencoding="A,\ B" style="vertical-align:-5px"><img src="" width="34.5" height="17.5" /></span><span> 是关于腿长 </span><span style="font-family: STIXGeneral, STIXGeneral-webfont, serif; font-style: italic; font-weight: 400; color: rgb(0, 0, 0);">l</span><span> 的函数，为了降低运算量，在工作空间内采用多项式拟合的方式得到状态反馈矩阵 K</span></div><div  class = 'S5'><span texencoding="K(l_l,l_r)=K_{K,1}+K_{K,2}l_l+K_{K,3}l_l^2+K_{K,4}l_r+K_{K,5}l_r^2+K_{K,6}l_ll_r" style="vertical-align:-8px"><img src="" width="352" height="21.5" /></span><span>    (3.5)</span></div><h3  class = 'S9' id = 'H_913C7C83' ><span>3.2 腿部控制器（Leg Controller）</span></h3><div  class = 'S5'><span>控制目标：调节腿部支持力控制左右腿长度，使机体保持特定高度&amp;滚转角，并起到悬挂减震的作用</span></div><div  class = 'S5' id = 'H_FFF298EC' ><span>输入：机体目标滚转角(roll) &amp; 目标腿长(平均值) </span><span texencoding="(\psi_{d},l_d)" style="vertical-align:-6px"><img src="" width="45" height="19.5" /></span></div><div  class = 'S5'><span>输出：左右腿支持力 </span><span texencoding="(F_{bl.l},F_{bl,r})" style="vertical-align:-6px"><img src="" width="67" height="19.5" /></span></div><div  class = 'S5'><span>反馈：机体滚转角 &amp; 左右腿长度 </span><span texencoding="(\psi_b,l_l,l_r)" style="vertical-align:-6px"><img src="" width="56.5" height="19.5" /></span></div><div  class = 'S5'><span>控制器：单环PID控制（PD模拟弹簧阻尼系统，I消除前馈误差）+前馈（重力+侧向惯性力矩补偿）</span></div><div  class = 'S5'><span>重力补偿前馈</span></div><div  class = 'S5'><span texencoding="F_{bl,gravity}=(\frac{1}{2}m_b+\eta_lm_l)g" style="vertical-align:-15px"><img src="" width="154.5" height="34" /></span><span>    (3.6)</span></div><div  class = 'S5'><span>侧向惯性力矩补偿前馈</span></div><div  class = 'S5'><span texencoding="F_{bl,inertial}=(\frac{1}{2}m_b+\eta_lm_l)\frac{l}{2R_l}\dot\phi\dot{s}" style="vertical-align:-17px"><img src="" width="189" height="36" /></span><span>    (3.7）</span></div><div  class = 'S5'><span>最终左右腿支持力</span></div><div  class = 'S5'><span texencoding="\left[\matrix{
F_{bl,l} \cr F_{bl,r}
}\right]=\left[\matrix{
1 &amp; 1 &amp; 1 &amp; -1 \cr -1 &amp; 1 &amp; 1 &amp; 1
}\right]\left[\matrix{
F_{\psi} \cr F_l \cr F_{bl,gravity} \cr F_{bl,inertial}
}\right]" style="vertical-align:-41px"><img src="" width="233" height="93" /></span><span>    (3.8)</span></div><div  class = 'S5'><span>在建模过程中将腿部机构进行了简化，控制量解耦为沿腿长方向的支持力和机体-腿关节处的转矩。而实际控制中腿部的执行器为2个关节电机，因此运动控制器输出的腿部转矩和腿部控制器输出的腿部支持力需要根据腿部机构动态静力学 (</span><a href = "./WBR_leg.html"><span>腿部机构分析式 (5.1)</span></a><span> ) 映射到腿部的关节电机力矩。此外，由于存在机械限位，为防止电机长时间工作在堵转状态，需要对每个关节电机通过额外的非线性控制律限制角度范围。</span></div><h2  class = 'S4' id = 'H_31305A8B' ><span>4 simulink仿真</span></h2><div  class = 'S5'><a href = "./WBR_control.slx"><span>WBR控制仿真</span></a></div><div  class = 'S5'></div>
<br>
<!-- 
##### SOURCE BEGIN #####
%% WBR控制
%% 0 准备工作
% <./WBR_modeling.mlx WBR系统建模分析>
% 
% <./WBR_leg.mlx WBR腿部五连杆机构分析>
%% 1 机器人参数
% 机器人质量、尺寸参数通过机械图纸测量得到，其中腿部惯量和质心位置简化近似为腿长 $l$ 的线性函数
% 
% 单侧腿平面转动惯量 
% 
% $I_l\approx0.4l+0.07\ (kg\cdot m^2)$    (1.1)
% 
% 腿部质心位置
% 
% $l_b\approx0.218l+0.075\ (m)$    (1.2)
% 
% $l_w=l-l_b\approx0.782l-0.075\ (m)$    (1.3)
%% 2 模型实例化
% 将机器人的具体参数代入模型中得到实例化的模型
% 2.1 状态空间模型
%% 
% 为降低计算量，通过多项式拟合得到状态方程中矩阵 A, B 关于腿长 $(l_l,l_r)$ 的近似函数。若计算资源紧张可利用矩阵 A, B 的稀疏特性进一步降低计算量。
% 
% $A(l_l,l_r)=K_{A,1}+K_{A,2}l_l+K_{A,3}l_l^2+K_{A,4}l_r+K_{A,5}l_r^2+K_{A,6}l_ll_r$    
% (2.1)
% 
% $B(l_l,l_r)=K_{B,1}+K_{B,2}l_l+K_{B,3}l_l^2+K_{B,4}l_r+K_{B,5}l_r^2+K_{B,6}l_ll_r$    
% (2.2)
% 2.2 腿部模型
% 由于腿部连杆正运动学计算较为复杂，因此在工作空间内采用多项式拟合得到正运动学的近似结果降低计算量
% 
% $x_e(\varphi_1,\varphi_2)=K_{xe,1}+K_{xe,2}\varphi_1+K_{xe,3}\varphi_2+K_{xe,4}\varphi_1^2+K_{xe,5}\varphi_2^2+K_{xe,6}\varphi_1\varphi_2$    
% (2.3)
% 
% $y_e(\varphi_1,\varphi_2)=K_{ye,1}+K_{ye,2}\varphi_1+K_{ye,3}\varphi_2+K_{ye,4}\varphi_1^2+K_{ye,5}\varphi_2^2+K_{ye,6}\varphi_1\varphi_2$    
% (2.4)
%% 3 控制器设计
% 机器人控制解耦为运动控制（Locomotion Controller）和腿部控制（Leg Controller）两部分。运动控制器通过调节驱动轮转矩和腿部关节转矩使机器人保持平衡和控制机器人移动、旋转；腿部控制器通过调节腿部支持力控制腿长，使机器人保持滚转（roll）方向稳定，并起到主动悬挂的作用。
% 3.1 运动控制器（Locomotion Controller）
% 根据状态空间模型设计状态反馈控制器
% 
% 状态反馈控制器
% 
% $\mathbf{u}=K_{4\times10}(\mathbf{x}_d-\mathbf{x})$    (3.1)
% 
% 状态反馈矩阵 K 通过 Linear Quadratic Regulator (LQR) 计算得到，代价函数
% 
% $J=\int_{0}^{\infty}\left(x^TQx+u^TRu\right)dx$    (3.2)
% 
% 根据 LQR 可以得到状态反馈矩阵 K
% 
% $K=R^{-1}B^TP$    (3.3)
% 
% 其中 P 满足 Riccati 方程
% 
% $A^TP+PA+PBR^{-1}B^TP+Q=0$    (3.4)
% 
% 式 (3.4) 中，$A,\ B$ 是关于腿长 $l$ 的函数，为了降低运算量，在工作空间内采用多项式拟合的方式得到状态反馈矩阵 K
% 
% $K(l_l,l_r)=K_{K,1}+K_{K,2}l_l+K_{K,3}l_l^2+K_{K,4}l_r+K_{K,5}l_r^2+K_{K,6}l_ll_r$    
% (3.5)
% 3.2 腿部控制器（Leg Controller）
% 控制目标：调节腿部支持力控制左右腿长度，使机体保持特定高度&滚转角，并起到悬挂减震的作用
% 
% 输入：机体目标滚转角(roll) & 目标腿长(平均值) $(\psi_{d},l_d)$
% 
% 输出：左右腿支持力 $(F_{bl.l},F_{bl,r})$
% 
% 反馈：机体滚转角 & 左右腿长度 $(\psi_b,l_l,l_r)$
% 
% 控制器：单环PID控制（PD模拟弹簧阻尼系统，I消除前馈误差）+前馈（重力+侧向惯性力矩补偿）
% 
% 重力补偿前馈
% 
% $F_{bl,gravity}=(\frac{1}{2}m_b+\eta_lm_l)g$    (3.6)
% 
% 侧向惯性力矩补偿前馈
% 
% $F_{bl,inertial}=(\frac{1}{2}m_b+\eta_lm_l)\frac{l}{2R_l}\dot\phi\dot{s}$    
% (3.7）
% 
% 最终左右腿支持力
% 
% $\left[\matrix{F_{bl,l} \cr F_{bl,r}}\right]=\left[\matrix{1 & 1 & 1 & -1 
% \cr -1 & 1 & 1 & 1}\right]\left[\matrix{F_{\psi} \cr F_l \cr F_{bl,gravity} 
% \cr F_{bl,inertial}}\right]$    (3.8)
% 
% 在建模过程中将腿部机构进行了简化，控制量解耦为沿腿长方向的支持力和机体-腿关节处的转矩。而实际控制中腿部的执行器为2个关节电机，因此运动控制器输出的腿部转矩和腿部控制器输出的腿部支持力需要根据腿部机构动态静力学 
% (<./WBR_leg.mlx 腿部机构分析式 (5.1)> ) 映射到腿部的关节电机力矩。此外，由于存在机械限位，为防止电机长时间工作在堵转状态，需要对每个关节电机通过额外的非线性控制律限制角度范围。
%% 4 simulink仿真
% <./WBR_control.slx WBR控制仿真>
% 
%
##### SOURCE END #####
-->
</div></body></html>